{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "**A Language Agent for Autonomous Driving**\n",
      "Role: You are the brain of an autonomous vehicle (a.k.a. ego-vehicle). In this step, you need to extract necessary information from the driving scenario. The information you extracted must be useful to the next-step motion planning. \n",
      "\n",
      "Necessary information might include the following:\n",
      "- Detections: The detected objects that you need to pay attention to.\n",
      "- Predictions: The estimated future motions of the detected objects. \n",
      "- Maps: Map information includes traffic lanes and road boundaries.\n",
      "- Occunpancy: Occupancy implies whether a location has been occupied by other objects.\n",
      "\n",
      "Task\n",
      "- You should think about what types of information (Detections, Predictions, Maps, Occupancy) you need to extract from the driving scenario.\n",
      "- Detections and Predictions are quite important for motion planning. You should call at least one of them if necessary.\n",
      "- Maps information are also important. You should pay more attention to road shoulder and lane divider information to your current ego-vehicle location.\n",
      "- I will guide you through the thinking process step by step.\n",
      "\n",
      "*****Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,0.92)\n",
      " - Heading Angular Velocity (v_yaw): (0.00)\n",
      " - Acceleration (ax,ay): (-0.00,-0.50)\n",
      " - Can Bus: (-0.74,0.14)\n",
      " - Heading Speed: (0.95)\n",
      " - Steering: (-0.02)\n",
      "Historical Trajectory (last 2 seconds): [(-0.07,-6.43), (-0.05,-4.34), (-0.02,-2.32), (-0.01,-0.91)]\n",
      "Mission Goal: FORWARD\n",
      "\n",
      "\n",
      "\n",
      "Do you need to perform detections from the driving scenario?\n",
      "Please answer YES or NO.\n",
      "\n",
      "YES\n",
      "You can execute one of the following functions to get object detection results (don't execute functions that have been used before):\n",
      "- get_leading_object_detection() #Get the detection of the leading object, the function will return the leading object id and its position and size. If there is no leading object, return None\n",
      "- get_object_detections_in_range(x_start, x_end, y_start, y_end) #Get the detections of the objects in a given range (x_start, x_end)*(y_start, y_end)m^2, the function will return a list of object ids and their positions and sizes. If there is no object, return None\n",
      "- get_surrounding_object_detections() #Get the detections of the surrounding objects in a 20m*20m range, the function will return a list of surroundind object ids and their positions and sizes. If there is no surrounding object, return None\n",
      "- get_front_object_detections() #Get the detections of the objects in front of you in a 10m*20m range, the function will return a list of front object ids and their positions and sizes. If there is no front object, return None\n",
      "- get_all_object_detections() #Get the detections of all objects in the whole scene, the function will return a list of object ids and their positions and sizes. Always avoid using this function if there are other choices.\n",
      "\n",
      "None\n",
      "get_front_object_detections\n",
      "{}\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 2, position: (4.36, 9.56), size: (1.86, 4.72)\n",
      "Front object detected, object type: car, object id: 3, position: (-3.70, 13.08), size: (2.01, 4.92)\n",
      "\n",
      "\n",
      "Do you need to perform future trajectory predictions for the detected objects?\n",
      "Please answer YES or NO.\n",
      "\n",
      "YES\n",
      "You can execute one of the following functions to get object future trajectory predictions (don't execute functions that have been used before):\n",
      "- get_leading_object_future_trajectory() #Get the predicted future trajectory of the leading object, the function will return a trajectory containing a series of waypoints. If there is no leading vehicle, return None\n",
      "- get_future_trajectories_for_specific_objects(object_ids) #Get the future trajectories of specific objects (specified by a List of object ids), the function will return trajectories for each object. If there is no object, return None\n",
      "- get_future_trajectories_in_range(x_start, x_end, y_start, y_end) #Get the future trajectories where any waypoint in this trajectory falls into a given range (x_start, x_end)*(y_start, y_end)m^2, the function will return each trajectory that satisfies the condition. If there is no trajectory satisfied, return None\n",
      "- get_future_waypoint_of_specific_objects_at_timestep(object_ids, timestep) #Get the future waypoints of specific objects at a specific timestep, the function will return a list of waypoints. If there is no object or the object does not have a waypoint at the given timestep, return None\n",
      "- get_all_future_trajectories() #Get the predicted future trajectories of all objects in the whole scene, the function will return a list of object ids and their future trajectories. Always avoid using this function if there are other choices.\n",
      "\n",
      "None\n",
      "get_future_trajectories_for_specific_objects\n",
      "{'object_ids': [2, 3]}\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(4.36, 9.56), (4.36, 9.56), (4.36, 9.57), (4.36, 9.57), (4.36, 9.56), (4.36, 9.56)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-2.66, 13.82), (-1.69, 14.79), (-0.99, 16.13), (-0.25, 17.73), (0.19, 19.42), (0.57, 21.35)]\n",
      "\n",
      "\n",
      "Do you need to get occupancy information for this driving scenario?\n",
      "Please answer YES or NO.\n",
      "\n",
      "NO\n",
      "\n",
      "Do you need to get map information for this driving scenario?\n",
      "Please answer YES or NO.\n",
      "\n",
      "YES\n",
      "You can execute one of the following functions to get map information (don't execute functions that have been used before):\n",
      "- get_drivable_at_locations(locations) #Get the drivability at the locations [(x_1, y_1), ..., (x_n, y_n)]. If the location is out of the map scope, return None\n",
      "- get_lane_category_at_locations(locations, return_score) #Get the lane category at the locations [(x_1, y_1), ..., (x_n, y_n)]. If the location is out of the map scope, return None\n",
      "- get_distance_to_shoulder_at_locations(locations) #Get the distance to both sides of road shoulders at the locations [(x_1, y_1), ..., (x_n, y_n)]. If the location is out of the map scope, return None\n",
      "- get_current_shoulder() #Get the distance to both sides of road shoulders for the current ego-vehicle location.\n",
      "- get_distance_to_lane_divider_at_locations(locations) #Get the distance to both sides of road lane_dividers at the locations [(x_1, y_1), ..., (x_n, y_n)]. If the location is out of the map scope, return None\n",
      "- get_current_lane_divider() #Get the distance to both sides of road lane_dividers for the current ego-vehicle location\n",
      "- get_nearest_pedestrian_crossing() #Get the location of the nearest pedestrian crossing to the ego-vehicle. If there is no such pedestrian crossing, return None\n",
      "\n",
      "None\n",
      "get_current_shoulder\n",
      "{}\n",
      "Distance to both sides of road shoulders of current ego-vehicle location:\n",
      "Current ego-vehicle's distance to left shoulder is 7.5m and right shoulder is 4.0m\n",
      "\n",
      "*****Perception Results:*****\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 2, position: (4.36, 9.56), size: (1.86, 4.72)\n",
      "Front object detected, object type: car, object id: 3, position: (-3.70, 13.08), size: (2.01, 4.92)\n",
      "\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(4.36, 9.56), (4.36, 9.56), (4.36, 9.57), (4.36, 9.57), (4.36, 9.56), (4.36, 9.56)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-2.66, 13.82), (-1.69, 14.79), (-0.99, 16.13), (-0.25, 17.73), (0.19, 19.42), (0.57, 21.35)]\n",
      "\n",
      "Distance to both sides of road shoulders of current ego-vehicle location:\n",
      "Current ego-vehicle's distance to left shoulder is 7.5m and right shoulder is 4.0m\n",
      "\n",
      "\n",
      "\n",
      "**A Language Agent for Autonomous Driving**\n",
      "Role: You are the brain of an autonomous vehicle (a.k.a. ego-vehicle). In this step, you need to retrieve the most similar past driving experience to help decision-making.\n",
      "\n",
      "Task\n",
      "- You will receive the current driving scenario.\n",
      "- You will also receive several past driving experiences.\n",
      "- You should decide ONLY ONE experience that is most similar to the current scenario based on the information provided.\n",
      "- Please answer ONLY the index (e.g., 0, 1, 2) of the most similar experience.\n",
      "\n",
      "** Current Scenario: **:\n",
      "*****Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,0.92)\n",
      " - Heading Angular Velocity (v_yaw): (0.00)\n",
      " - Acceleration (ax,ay): (-0.00,-0.50)\n",
      " - Can Bus: (-0.74,0.14)\n",
      " - Heading Speed: (0.95)\n",
      " - Steering: (-0.02)\n",
      "Historical Trajectory (last 2 seconds): [(-0.07,-6.43), (-0.05,-4.34), (-0.02,-2.32), (-0.01,-0.91)]\n",
      "Mission Goal: FORWARD\n",
      "*****Perception Results:*****\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 2, position: (4.36, 9.56), size: (1.86, 4.72)\n",
      "Front object detected, object type: car, object id: 3, position: (-3.70, 13.08), size: (2.01, 4.92)\n",
      "\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(4.36, 9.56), (4.36, 9.56), (4.36, 9.57), (4.36, 9.57), (4.36, 9.56), (4.36, 9.56)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-2.66, 13.82), (-1.69, 14.79), (-0.99, 16.13), (-0.25, 17.73), (0.19, 19.42), (0.57, 21.35)]\n",
      "\n",
      "Distance to both sides of road shoulders of current ego-vehicle location:\n",
      "Current ego-vehicle's distance to left shoulder is 7.5m and right shoulder is 4.0m\n",
      "\n",
      "Found 3 relevant experiences:\n",
      "** Past Driving Experience 1: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (0.00,1.07)\n",
      " - Heading Angular Velocity (v_yaw): (-0.00)\n",
      " - Acceleration (ax,ay): (-0.02,-0.43)\n",
      " - Can Bus: (-0.67,0.03)\n",
      " - Heading Speed: (1.00)\n",
      " - Steering: (0.13)\n",
      "Historical Trajectory (last 2 seconds): [(-0.16,-6.66), (-0.08,-4.46), (-0.03,-2.55), (-0.00,-1.06)]\n",
      "Mission Goal: FORWARD\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(-1.13, -13.82), (-1.09, -12.18), (-1.05, -10.66), (-0.98, -9.22), (-0.98, -7.96), (-0.93, -6.74)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-25.19, -17.79), (-25.19, -17.79), (-25.18, -17.78), (-25.18, -17.78), (-25.18, -17.78), (-25.17, -17.78)]\n",
      "** Past Driving Experience 2: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,0.97)\n",
      " - Heading Angular Velocity (v_yaw): (0.00)\n",
      " - Acceleration (ax,ay): (-0.01,-0.46)\n",
      " - Can Bus: (-0.68,0.11)\n",
      " - Heading Speed: (1.16)\n",
      " - Steering: (0.04)\n",
      "Historical Trajectory (last 2 seconds): [(-0.06,-6.03), (-0.04,-4.10), (-0.02,-2.40), (-0.01,-0.97)]\n",
      "Mission Goal: FORWARD\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 0, position: (-0.58, 9.54), size: (1.75, 3.99)\n",
      "Front object detected, object type: car, object id: 2, position: (3.57, 7.27), size: (1.88, 4.49)\n",
      "Front object detected, object type: truck, object id: 3, position: (-0.08, 19.97), size: (2.93, 9.04)\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(3.59, 12.47), (3.61, 17.78), (3.68, 23.26), (3.70, 28.79), (3.66, 34.46), (3.65, 40.07)]\n",
      "Object type: truck, object id: 3, future waypoint coordinates in 3s: [(-0.08, 19.96), (-0.08, 19.93), (-0.09, 19.93), (-0.09, 19.92), (-0.08, 19.92), (-0.09, 19.91)]\n",
      "** Past Driving Experience 3: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,1.06)\n",
      " - Heading Angular Velocity (v_yaw): (-0.00)\n",
      " - Acceleration (ax,ay): (0.00,-0.32)\n",
      " - Can Bus: (-0.77,0.04)\n",
      " - Heading Speed: (1.06)\n",
      " - Steering: (-0.01)\n",
      "Historical Trajectory (last 2 seconds): [(-0.08,-6.01), (-0.05,-4.12), (-0.04,-2.43), (-0.02,-1.06)]\n",
      "Mission Goal: FORWARD\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 1, position: (-3.08, 2.56), size: (1.88, 4.52)\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.31), (-18.54, -24.31)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-3.26, -31.78), (-3.43, -28.27), (-3.36, -25.04), (-3.45, -22.14), (-3.46, -19.62), (-3.51, -17.45)]\n",
      "Please return the index 1-3 of the most similar experience: \n",
      "Memory-GPT response: 2\n",
      "*****Chain of Thoughts Reasoning:*****\n",
      "Thoughts:\n",
      " - Notable Objects: car at (4.36,9.56), moving to (4.36,9.57) at 0.5 second\n",
      "   Potential Effects: within the safe zone of the ego-vehicle at 0.5 second\n",
      " - Notable Objects: car at (-3.70,13.08), moving to (-2.66,13.82) at 0.5 second\n",
      "   Potential Effects: within the safe zone of the ego-vehicle at 0.5 second\n",
      "Driving Plan: MOVE FORWARD WITH A CONSTANT SPEED\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "sys.path.append(\"../..\")\n",
    "from agentdriver.memory.memory_agent import MemoryAgent\n",
    "from agentdriver.perception.perception_agent import PerceptionAgent\n",
    "from agentdriver.reasoning.reasoning_agent import ReasoningAgent\n",
    "from agentdriver.llm_core.api_keys import OPENAI_ORG, OPENAI_API_KEY\n",
    "\n",
    "import openai\n",
    "openai.organization = OPENAI_ORG\n",
    "openai.api_key = OPENAI_API_KEY\n",
    "\n",
    "from pathlib import Path\n",
    "\n",
    "data_path = Path('../../data/')\n",
    "\n",
    "perception_agent = PerceptionAgent(token=\"0a0d6b8c2e884134a3b48df43d54c36a\", split=\"val\", data_path=data_path, verbose=True)\n",
    "ego_prompts, perception_prompts, working_memory = perception_agent.run()\n",
    "\n",
    "memory_agent = MemoryAgent(data_path=data_path, verbose=True, compare_perception=True)\n",
    "commonsense_mem, experience_mem = memory_agent.run(working_memory)\n",
    "\n",
    "reasoning_agent = ReasoningAgent(verbose=True)\n",
    "reasoning = reasoning_agent.run(perception_agent.data_dict, ego_prompts+perception_prompts, working_memory)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "llmagent",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
